#include <iostream>
#include <vector>
#include <cmath>

namespace elastic_band{
    class Point2D
    {
    public:
        Point2D(double _x = 0.0,double _y = 0.0):x(_x),y(_y){};
        ~Point2D();

        double x,y;
        double distance(const Point2D &other) const{
            return std::sqrt((x-other.x) *(x-other.x) + (y-other.y)*(y-other.y));
        }
    };

    class ElasticBand{
        private:
            std::vector<Point2D> path_points;
            double repulsive_gain;
            double attractive_gain;
            double min_obstacle_distance;
        public:
            ElasticBand(const std::vector<Point2D>& initial_path,double rep_gain,double att_gain,double min_obst_dist):
            path_points(initial_path),repulsive_gain(rep_gain),attractive_gain(att_gain),min_obstacle_distance(min_obst_dist){}

            void update_with_obstacles(const std::vector<Point2D> &obstacles,size_t max_iterations = 100){
                for(size_t counter = 0;counter < max_iterations;++counter){
                    for(size_t i = 1;i<path_points.size()-1;++i){
                        Point2D repulsive_force(0,0);
                        Point2D attractive_force(0,0);

                        // 计算斥力
                        for(auto obstacle : obstacles){
                            double d = path_points[i].distance(obstacle);
                            if(d<min_obstacle_distance){
                                double force_magnitude = repulsive_gain * (1/d - 1/min_obstacle_distance)/(d*d);
                                repulsive_force.x += force_magnitude * (path_points[i].x - obstacle.x);
                                repulsive_force.y += force_magnitude * (path_points[i].y - obstacle.y);
                            }
                        }

                        // 计算引力
                        Point2D path_difference = {path_points[i+1].x - path_points[i-1].x,
                                                    path_points[i+1].y - path_points[i-1].x};
                        double d = path_points[i].distance(path_points[i-1]);
                        attractive_force.x = attractive_gain * (path_difference.x/d - (path_points[i].x - path_points[i-1].x));
                        attractive_force.y = attractive_gain * (path_difference.y/d - (path_points[i].y - path_points[i-1].y));
                    
                        // 更新节点位置
                        path_points[i].x += repulsive_force.x + attractive_force.x;
                        path_points[i].y += repulsive_force.y + attractive_force.y;
                    }
                }
            }

            const std::vector<Point2D> &get_path_points() const{
                return path_points;
            }
    };
}